/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
#include <iostream>
#include <boost/thread/thread.hpp>
#include <pcl/common/common_headers.h>
#include <pcl/common/common_headers.h>
#include <pcl/io/pcd_io.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/console/parse.h>
#include <pcl/filters/voxel_grid.h>

#include <pcl/point_types.h>

using namespace pcl;
using namespace pcl::visualization;

typedef pcl::PointXYZRGB PointType;

///PointXYZRGB
/**
int color = *reinterpret_cast<const int*>(&(input_->points[i].rgb));
int r = (0xff0000 & color) >> 16;
int g = (0x00ff00 & color) >> 8;
int b = 0x0000ff & color; 
*/
int
main (int argc, char** argv)
{
	if(argc !=2){
		fprintf(stderr,"\nUsage: ./reader [3dDatafile]\n");
		exit(1);
	}
	
	char *filename = argv[1];
	FILE *f = fopen(filename,"rt");
	
	if(f==NULL){
		fprintf(stderr,"Unable to open file %s\n",filename);
		return 0;
	}
	
	
	pcl::PointCloud<PointType>::Ptr point_cloud_ptr (new pcl::PointCloud<PointType>);
	pcl::PointCloud<pcl::PointXYZRGB>& cloud = *point_cloud_ptr;
	
	pcl::PointXYZRGB p;
	uint32_t r,g,b;
	while(!feof(f)){
		fscanf(f,"%f %f %f %d %d %d\n",&p.x,&p.y,&p.z,&r,&g,&b);
		p.z = -p.z;
		uint32_t rgb = (static_cast<uint32_t>(r) << 16 |
		static_cast<uint32_t>(g) << 8 | static_cast<uint32_t>(b));
		p.rgb = *reinterpret_cast<float*>(&rgb);
		
		
		//int color = (r<<16) | (g<<8) | b;
		//p.rgb = static_cast<float>(color);
		cloud.push_back(p);
	}
	fclose(f);
	std::string outfile;
	outfile=filename;
	outfile+=".pcd";
	fprintf(stderr,"Saving output to .pcd format\n");
	pcl::io::savePCDFileASCII (outfile.c_str(), cloud);
	
	// --------------------------------------------
	// -----Open 3D viewer and add point cloud-----
	// --------------------------------------------
	PCLVisualizer viewer ("3D Viewer");
	viewer.setBackgroundColor (0, 0, 0);
	PointCloudColorHandlerRGBField<PointType> chandler(point_cloud_ptr);
	
	viewer.addPointCloud<PointType> (point_cloud_ptr, chandler, "sample cloud");
	viewer.setPointCloudRenderingProperties (PCL_VISUALIZER_POINT_SIZE, 3, "sample cloud");
	viewer.addCoordinateSystem (1.0);
	viewer.initCameraParameters ();
	
	//--------------------
	// -----Main loop-----
	//--------------------
	while (!viewer.wasStopped ())
	{
		viewer.spinOnce (100);
		boost::this_thread::sleep (boost::posix_time::microseconds (100000));
	}
	
	
	

return 0;
}